/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : freertos.c
 * Description        : Code for freertos applications
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "BMI088.h"
#include "MyCAN.h"
#include "Kalman.h"
#include "usbd_cdc_if.h"
#include "DriversStruct.h"
#include "DriversUse.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
TaskHandle_t xUSBTaskHandle;    // USB虚拟串口
TaskHandle_t xAutoAimingHandle; // 自瞄任务
TaskHandle_t xScanningHandle;   // 扫描任务
TaskHandle_t xUpdateTaskHandle; // 始终都需要运行的任务

PlatformTypedef Platform;         // 包含两个电机的云台
BMI088_ReceiveDataTypedef BMI088; // 陀螺仪
CameraTypedef Camera;             // 与视觉相机的通信
DebugValueTypedef DebugValue;     // debug有关的变量

float step_1 = 0.2;  // 步进值 单位°
float step_2 = 0.02; // 步进值 单位°

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
    .name       = "defaultTask",
    .stack_size = 128 * 4,
    .priority   = (osPriority_t)osPriorityNormal,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void USBTask(void *argument);
void AutoAimingTask(void *argument);
void ScanningTask(void *argument);
void UpdateTask(void *argument);

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
 * @brief  FreeRTOS initialization
 * @param  None
 * @retval None
 */
void MX_FREERTOS_Init(void)
{
    /* USER CODE BEGIN Init */
    Userf_AllInit();
    /* USER CODE END Init */

    /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
    /* USER CODE END RTOS_MUTEX */

    /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
    /* USER CODE END RTOS_SEMAPHORES */

    /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
    /* USER CODE END RTOS_TIMERS */

    /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
    /*            USB           */
    xTaskCreate(USBTask,          // 函数指针
                "USBTask",        // 任务名字
                256,              // 栈大小，单位word，10表示40字节
                NULL,             // 调用任务函数是传入的参数
                osPriorityNormal, // 优先级
                &xUSBTaskHandle); // 任务句柄

    /*
    自瞄模式
    xTaskCreate(AutoAimingTask,		// 函数指针
          "AutoAimingTask",	// 任务名字
          256,				// 栈大小，单位word，10表示40字节
          NULL,				// 调用任务函数是传入的参数
          osPriorityNormal1,	// 优先级
          xAutoAimingHandle); // 任务句柄

    扫描模式
    xTaskCreate(ScanningTask,	   // 函数指针
          "ScanningTask",	   // 任务名字
          256,			   // 栈大小，单位word，10表示40字节
          NULL,			   // 调用任务函数是传入的参数
          osPriorityNormal1, // 优先级
          &xScanningHandle); // 任务句柄

    */

    xTaskCreate(ScanningTask,      // 函数指针
                "ScanningTask",    // 任务名字
                256,               // 栈大小，单位word，10表示40字节
                NULL,              // 调用任务函数是传入的参数
                osPriorityNormal1, // 优先级
                &xScanningHandle); // 任务句柄

    /*          Update         */
    xTaskCreate(UpdateTask,          // 函数指针
                "UpdateTask",        // 任务名字
                256,                 // 栈大小，单位word，10表示40字节
                NULL,                // 调用任务函数是传入的参数
                osPriorityNormal,    // 优先级
                &xUpdateTaskHandle); // 任务句柄

    /* USER CODE END RTOS_QUEUES */

    /* Create the thread(s) */
    /* creation of defaultTask */
    defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

    /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
    /* USER CODE END RTOS_THREADS */

    /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */
    /* USER CODE END RTOS_EVENTS */
}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
 * @brief  Function implementing the defaultTask thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
    /* init code for USB_DEVICE */
    MX_USB_DEVICE_Init();
    /* USER CODE BEGIN StartDefaultTask */
    /* Infinite loop */
    for (;;) {
        osDelay(1);
    }
    /* USER CODE END StartDefaultTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void USBTask(void *argument)
{
    while (1) {
        // usb_printf("%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\n",
        //            Platform.motor1.g_Angle_pid.Setpoint,
        //            Platform.motor1.ReceiveData.Angle,
        //            BMI088.roll,
        //            Platform.motor2.g_Angle_pid.Setpoint,
        //            Platform.motor2.ReceiveData.Angle,
        //            BMI088.yaw); // 发送数据

        Userf_SendLedTypeToCamera();
        vTaskDelay(1);
    }
}
void AutoAimingTask(void *argument)
{
    while (1)
    {
        /* 边缘判断，接收数据 */
        if (Platform.motor1.ReceiveData.Angle >= MAX_MOTOR_ANGLE &&
            Camera.pitch.angle >= BMI088.pitch) // 编码值大于限位值，并且相机角度仍有要变大的趋势
        {
            DebugValue.motor1_Edge = 1;
            Platform.motor1.g_Angle_pid.Setpoint = MAX_MOTOR_ANGLE + 0.2;
        }
        else if (Platform.motor1.ReceiveData.Angle <= -MAX_MOTOR_ANGLE &&
                Camera.pitch.angle <= BMI088.pitch) // 编码值小于限位值，并且相机角度仍有要变小的趋势
        {
            DebugValue.motor1_Edge = 1;
            Platform.motor1.g_Angle_pid.Setpoint = -MAX_MOTOR_ANGLE - 0.2;
        }
        else
        {
            DebugValue.motor1_Edge = 0;
            Platform.motor1.g_Angle_pid.Setpoint = Camera.pitch.angle;
        }

        if(Platform.motor2.ReceiveData.Angle >= MAX_MOTOR_ANGLE &&
            Camera.yaw.angle >= BMI088.yaw) // 编码值大于限位值，并且相机角度仍有要变大的趋势
        {
            DebugValue.motor2_Edge = 1;
            Platform.motor2.g_Angle_pid.Setpoint = MAX_MOTOR_ANGLE + 0.2;
        }
        else if (Platform.motor2.ReceiveData.Angle <= -MAX_MOTOR_ANGLE &&
                Camera.yaw.angle <= BMI088.yaw)
        {
            DebugValue.motor2_Edge = 1;
            Platform.motor2.g_Angle_pid.Setpoint = -MAX_MOTOR_ANGLE - 0.2;
        }
        else
        {
            DebugValue.motor2_Edge = 0;
            Platform.motor2.g_Angle_pid.Setpoint = Camera.yaw.angle;
        }

        /* 进行PID并发送给电机 */
        if(DebugValue.motor1_Edge == 1)
        {
            Userf_DoubleLoopPIDControl_Motor1_UseMotorAngle();
        }
        else
        {
            Userf_DoubleLoopPIDControl_Motor1();
        }
        Userf_MotorActivit(&Platform.motor1);

        if(DebugValue.motor2_Edge == 1)
        {
            Userf_DoubleLoopPIDControl_Motor1_UseMotorAngle();
        }
        else
        {
            Userf_DoubleLoopPIDControl_Motor2();
        }
        Userf_MotorActivit(&Platform.motor2);
        vTaskDelay(1);
    }
}

void ScanningTask(void *argument)
{
    while (1) {
        /*翻转步进值*/
        if (Platform.motor1.ReceiveData.Angle <= -MAX_MOTOR_ANGLE)
        {
            step_1 = 0.2;
        } 
        else if (Platform.motor1.ReceiveData.Angle >= MAX_MOTOR_ANGLE) 
        {
            step_1 = -0.2;
        }

        if (Platform.motor2.ReceiveData.Angle <= -MAX_MOTOR_ANGLE)
        {
            step_2 = 0.02;
        } 
        else if (Platform.motor2.ReceiveData.Angle >= MAX_MOTOR_ANGLE) 
        {
            step_2 = -0.02;
        }

        // Platform.motor1.g_Angle_pid.Setpoint = Userf_SummorSin_Pitch();
        // Platform.motor2.g_Angle_pid.Setpoint = Userf_SummorSin_Yaw();

        Platform.motor1.g_Angle_pid.Setpoint += step_1;
        Platform.motor2.g_Angle_pid.Setpoint += step_2;

        /*目标值限制*/
        if (Platform.motor1.g_Angle_pid.Setpoint >= MAX_MOTOR_ANGLE + 3) 
        {
            Platform.motor1.g_Angle_pid.Setpoint = MAX_MOTOR_ANGLE + 2.8;
            step_1                               = 0;
        } 
        else if (Platform.motor1.g_Angle_pid.Setpoint <= -MAX_MOTOR_ANGLE - 3) 
        {
            Platform.motor1.g_Angle_pid.Setpoint = -MAX_MOTOR_ANGLE - 2.8;
            step_1                               = 0;
        }

        if (Platform.motor2.g_Angle_pid.Setpoint >= MAX_MOTOR_ANGLE + 0.3) 
        {
            Platform.motor2.g_Angle_pid.Setpoint = MAX_MOTOR_ANGLE + 0.28;
            step_2                               = 0;
        } 
        else if (Platform.motor2.g_Angle_pid.Setpoint <= -MAX_MOTOR_ANGLE - 0.3) 
        {
            Platform.motor2.g_Angle_pid.Setpoint = -MAX_MOTOR_ANGLE - 0.28;
            step_2                               = 0;
        }

        /*进行PID并发送*/
        Userf_DoubleLoopPIDControl_UseMotorAngle();
        // Userf_DoubleLoopPIDControl_Motor1_NOF();
        Userf_MotorActivit(&Platform.motor1);
        // Userf_DoubleLoopPIDControl_Motor2_NOF();
        Userf_MotorActivit(&Platform.motor2);
        vTaskDelay(1);
    }
}

void UpdateTask(void *argument)
{
    while (1) {
        //		kalman_filter_update(&q, &BMI088, &timer);
        //
        //        angles= quaternion_to_euler(q);	     // 四元数转欧拉角
        // Userf_MotorActivit(&Platform.motor1);
        // Userf_MotorActivit(&Platform.motor2);
        // Userf_DoubleLoopPIDControl();
        Userf_BMI088Update();
        Userf_MotorAngleProtect();
        if (Camera.isTarget != Camera.lastIsTarget) // 识别目标发生变化
        {
            if (Camera.isTarget == 0) // 更改任务
            {
                vTaskDelete(xAutoAimingHandle); // 自瞄变成扫描
                Userf_Reset_PIDSetPoint_Motor();//切换编码值为实际值的PID
                step_1 = 0.2;
                step_2 = 0.02;
                xTaskCreate(ScanningTask,      // 函数指针
                            "ScanningTask",    // 任务名字
                            256,               // 栈大小，单位word，10表示40字节
                            NULL,              // 调用任务函数是传入的参数
                            osPriorityNormal1, // 优先级
                            &xScanningHandle); // 任务句柄
            }
            else
            {
                vTaskDelete(xScanningHandle);   //扫描变成自瞄
                Userf_Reset_PIDSetPoint_BMI();   // 切换陀螺仪角度为实际值的PID
                DebugValue.motor1_Edge = 0;     // 重置记录边缘的变量
                DebugValue.motor2_Edge = 0;
                xTaskCreate(AutoAimingTask,      // 函数指针
                            "AutoAimingTask",    // 任务名字
                            256,                 // 栈大小，单位word，10表示40字节
                            NULL,                // 调用任务函数是传入的参数
                            osPriorityNormal1,   // 优先级
                            &xAutoAimingHandle); // 任务句柄
            }
            Camera.lastIsTarget = Camera.isTarget;
        }
    }
}

/* USER CODE END Application */
